//
// Created by jiangbin on 4/22/21.
//

#include <stdio.h>
#include <stdlib.h>
#include "../include/front_wheel.h"

void ahead(void)
{
    FILE * fp;
    char cmd;
    int i = 0;
    int pos = 190;
    int pwm = 0;

    fp = fopen("/sys/class/pwm/pwmchip6/pwm1/duty_cycle", "r+");
    if (fp == NULL){
        perror("fopen error");
        return ;
    }

    pos = 190;
    pwm = 2000 - pos;
    fseek(fp, 0L, SEEK_SET);
    fprintf(fp, "%d", pwm * 10000);
    fclose(fp);
}

void right(int angle)
{
    FILE * fp;
    int i = 0;
    int pos = 190;
    int pwm = 0;

    fp = fopen("/sys/class/pwm/pwmchip6/pwm1/duty_cycle", "r+");
    if (fp == NULL){
        perror("fopen error");
        return ;
    }

    fseek(fp, 0L, SEEK_SET);
    pos += 10*angle;

    if(pos < 125 || pos > 250){
        printf("limit value exceeded \n");
        pos = 190;
        pwm = 2000 - pos;
        fprintf(fp, "%d", pwm * 10000);
    }else{
        pwm = 2000 - pos;
        fprintf(fp, "%d", pwm * 10000);
    }
    fclose(fp);
}

void left(int angle)
{
    FILE * fp;
    int i = 0;
    int pos = 190;
    int pwm = 0;

    fp = fopen("/sys/class/pwm/pwmchip6/pwm1/duty_cycle", "r+");
    if (fp == NULL){
        perror("fopen error");
        return ;
    }

    fseek(fp, 0L, SEEK_SET);
    pos -= 10*angle;

    if(pos < 125 || pos > 250){
        printf("limit value exceeded \n");
        pos = 190;
        pwm = 2000 - pos;
        fprintf(fp, "%d", pwm * 10000);
    }else{
        pwm = 2000 - pos;
        fprintf(fp, "%d", pwm * 10000);
    }
    fclose(fp);
}